//-----------------------------------------------------------------------
//  This file is based on Microsoft Robotics Developer Studio Code Samples.
// 
//  Portions Copyright (C) Microsoft Corporation.  All rights reserved.
//  Updated service created by Trevor Taylor.
//  This code released under the terms of the 
//  Microsoft Public License (MS-PL, http://opensource.org/licenses/ms-pl.html)
//
//  $File: HiTechnicTouchMux.cs $ $Revision: 1 $
//-----------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Net;
using W3C.Soap;

using pxbrick = Microsoft.Robotics.Services.Sample.Lego.Nxt.Brick.Proxy;
using dssphttp = Microsoft.Dss.Core.DsspHttp;
using contactsensor = Microsoft.Robotics.Services.ContactSensor.Proxy;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using nxtcmd = Microsoft.Robotics.Services.Sample.Lego.Nxt.Commands;
using Microsoft.Robotics.Services.Sample.Lego.Nxt.Common;
using Microsoft.Robotics.Services.Sample.Lego.Nxt.Commands;

namespace Microsoft.Robotics.Services.Sample.HiTechnic.TouchMux
{    
    /// <summary>
    /// HiTechnic Touch Sensor Mux Service
    /// </summary>
    [Contract(Contract.Identifier)]
    [AlternateContract(contactsensor.Contract.Identifier)]
    [Description("Provides access to the HiTechnic Touch Sensor Mux.")]
    [DisplayName("HiTechnic Touch Sensor Mux")]
    // This is a link to the doc page
    [DssServiceDescription("http://mrdshitechnic.codeplex.com/wikipage?title=TouchMux")]
    [DssCategory(pxbrick.LegoCategories.NXT)]
    public class HiTechnicTouchMux : DsspServiceBase
    {
        /// <summary>
        /// Service State
        /// </summary>
        [InitialStatePartner(Optional = true, ServiceUri = ServicePaths.Store + "/hiTechnic.TouchMux.config.xml")]
        private TouchMuxState _state = new TouchMuxState();
        private contactsensor.ContactSensorArrayState _genericContactState = new contactsensor.ContactSensorArrayState();

        // Create a local ContactSensorArray for convenience
        //private contactsensor.ContactSensor _genericSensor = new contactsensor.ContactSensor();
        // The port number that this service is attached to
        private int _sensorPort = 0;

        /// <summary>
        /// Main Operations Port
        /// </summary>
        [ServicePort("/hitechnic/nxt/touchmux", AllowMultipleInstances=true)]
        private TouchMuxOperations _internalMainPort = new TouchMuxOperations();
        private TouchMuxOperations _reliableMainPort = null;

        // Support the standard contact sensor contract as well as the Touch Mux
        [AlternateServicePort("/contactsensor",
            AllowMultipleInstances = true,
            AlternateContract = contactsensor.Contract.Identifier)]
        private contactsensor.ContactSensorArrayOperations _genericContactPort = new contactsensor.ContactSensorArrayOperations();

        // The XSLT page for displaying the state in a web browser
        [EmbeddedResource("Microsoft.Robotics.Services.Sample.HiTechnic.Transforms.TouchMux.xslt")]
        private string _transform = string.Empty;

        /// <summary>
        /// Partner with the LEGO NXT Brick
        /// </summary>
        [Partner("brick",
            Contract = pxbrick.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate,
            Optional = false)]
        private pxbrick.NxtBrickOperations _legoBrickPort = new pxbrick.NxtBrickOperations();
        private pxbrick.NxtBrickOperations _legoBrickNotificationPort = new pxbrick.NxtBrickOperations();

        /// <summary>
        /// Subscription manager partner for Touch Sensor
        /// </summary>
        [Partner(Partners.SubscriptionManagerString, Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort();

        /// <summary>
        /// Subscription manager partner for generic contact sensor
        /// </summary>
        [Partner(Partners.SubscriptionManagerString + "/touchmux", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        submgr.SubscriptionManagerPort _genericSubMgrPort = new submgr.SubscriptionManagerPort();


        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public HiTechnicTouchMux(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }

        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            InitializeState();

			base.Start();

            base.MainPortInterleave.CombineWith(new Interleave(
                new ExclusiveReceiverGroup(
                    Arbiter.Receive<pxbrick.LegoSensorUpdate>(true, _legoBrickNotificationPort, NotificationHandler)
                ),
                new ConcurrentReceiverGroup()));

            // Set up the reliable port using DSS forwarder to ensure exception and timeout conversion to fault.
            _reliableMainPort = ServiceForwarder<TouchMuxOperations>(this.ServiceInfo.Service);
            _reliableMainPort.ConnectToBrick(_state);

        }

        /// <summary>
        /// Initialize State
        /// </summary>
        private void InitializeState()
        {
            bool save = false;

            if (_state == null)
            {
                save = true;
                _state = new TouchMuxState();
            }

            if (_state.PollingFrequencyMs == 0)
                _state.PollingFrequencyMs = Contract.DefaultPollingFrequencyMs;

            _state.TimeStamp = DateTime.MinValue;
            _state.Connected = false;
            _state.AllTouchSensors = 0;
            _state.TouchSensor1On = false;
            _state.TouchSensor2On = false;
            _state.TouchSensor3On = false;
            _state.TouchSensor4On = false;

            if (_genericContactState == null)
            {
                _genericContactState = new contactsensor.ContactSensorArrayState();
            }
            // Generic contact sensor uses List of sensors
            if (_genericContactState.Sensors == null)
            {
                _genericContactState.Sensors = new List<contactsensor.ContactSensor>();
            }
            // Make sure that there are four sensors in the list
            if (_genericContactState.Sensors.Count == 0)
            {
                _genericContactState.Sensors.Add(new contactsensor.ContactSensor(1, "Touch Sensor 1"));
                _genericContactState.Sensors.Add(new contactsensor.ContactSensor(2, "Touch Sensor 2"));
                _genericContactState.Sensors.Add(new contactsensor.ContactSensor(3, "Touch Sensor 3"));
                _genericContactState.Sensors.Add(new contactsensor.ContactSensor(4, "Touch Sensor 4"));
            }

            if (save)
                SaveState(_state);
        }

        private void NotificationHandler(pxbrick.LegoSensorUpdate update)
        {
            nxtcmd.LegoResponseGetInputValues inputValues =  new nxtcmd.LegoResponseGetInputValues(update.Body.CommandData);
            if (inputValues != null && inputValues.Success)
            {
                // Remember the last switch states
                int lastSwitches = _state.AllTouchSensors;

                // Calculate the touch sensor states from the analog value.
                // This is complicated and non-linear. See the HiTechnic docs.
                int value = 1023 - inputValues.RawValue;
                int switches = 339 * value;
                switches /= 1023 - value;
                switches += 5;
                switches /= 10;
                switches &= 0x0F;

                if (lastSwitches != switches || _state.TimeStamp == DateTime.MinValue)
                {
                    // This is for debugging only
                    //LogInfo(LogGroups.Console, "Switches: " + switches);
                    _state.AllTouchSensors = (byte)switches;
                    _state.TouchSensor1On = (switches & (int)TouchMuxSensorMasks.Sensor1) != 0;
                    _state.TouchSensor2On = (switches & (int)TouchMuxSensorMasks.Sensor2) != 0;
                    _state.TouchSensor3On = (switches & (int)TouchMuxSensorMasks.Sensor3) != 0;
                    _state.TouchSensor4On = (switches & (int)TouchMuxSensorMasks.Sensor4) != 0;
                    _state.TimeStamp = inputValues.TimeStamp;

                    // Send Touch Sensor Mux notification
                    SendNotification<TouchMuxUpdate>(_subMgrPort, _state);

                    // Send contact sensor array notification
                    SyncGenericContactState();
                    SendNotification<contactsensor.Replace>(_genericSubMgrPort, _genericContactState);

                    // Do an update for each Touch sensor as well, but only if changed
                    if ((switches & (int)TouchMuxSensorMasks.Sensor1) != (lastSwitches & (int)TouchMuxSensorMasks.Sensor1))
                        SendNotification<contactsensor.Update>(_genericSubMgrPort, _genericContactState.Sensors[0]);
                    if ((switches & (int)TouchMuxSensorMasks.Sensor2) != (lastSwitches & (int)TouchMuxSensorMasks.Sensor2))
                        SendNotification<contactsensor.Update>(_genericSubMgrPort, _genericContactState.Sensors[1]);
                    if ((switches & (int)TouchMuxSensorMasks.Sensor3) != (lastSwitches & (int)TouchMuxSensorMasks.Sensor3))
                        SendNotification<contactsensor.Update>(_genericSubMgrPort, _genericContactState.Sensors[2]);
                    if ((switches & (int)TouchMuxSensorMasks.Sensor4) != (lastSwitches & (int)TouchMuxSensorMasks.Sensor4))
                        SendNotification<contactsensor.Update>(_genericSubMgrPort, _genericContactState.Sensors[3]);
                }
            }
        }

        /// <summary>
        /// Sync the generic contact sensor state with our touch sensor.
        /// </summary>
        /// <returns></returns>
        private contactsensor.ContactSensorArrayState SyncGenericContactState()
        {
            // If the Contact Sensor Array has not been created there is a problem!
            if (_genericContactState.Sensors.Count != 4)
                throw(new Exception("Contact Sensor Array State is invalid"));

            // Copy the sensor info to the generic state (contact sensor array)
            _genericContactState.Sensors[0].Pressed = _state.TouchSensor1On;
            _genericContactState.Sensors[0].TimeStamp = _state.TimeStamp;
            _genericContactState.Sensors[1].Pressed = _state.TouchSensor2On;
            _genericContactState.Sensors[1].TimeStamp = _state.TimeStamp;
            _genericContactState.Sensors[2].Pressed = _state.TouchSensor3On;
            _genericContactState.Sensors[2].TimeStamp = _state.TimeStamp;
            _genericContactState.Sensors[3].Pressed = _state.TouchSensor4On;
            _genericContactState.Sensors[3].TimeStamp = _state.TimeStamp;

            return _genericContactState;
        }

        /// <summary>
        /// Get Touch Sensor Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetTouchMuxHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }

        /// <summary>
        /// Get Contact Sensor Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_genericContactPort")]
        public virtual IEnumerator<ITask> GetContatSensorHandler(contactsensor.Get get)
        {
            get.ResponsePort.Post(SyncGenericContactState());
            yield break;
        }

        /// <summary>
        /// HttpGet Touch Mux Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpGetTouchMuxHandler(dssphttp.HttpGet get)
        {
            get.ResponsePort.Post(new dssphttp.HttpResponseType(HttpStatusCode.OK, _state, _transform));
            yield break;
        }
        /*
        /// <summary>
        /// HttpGet AnalogSensor Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_genericAnalogPort")]
        public virtual IEnumerator<ITask> HttpGetAnalogSensorHandler(dssphttp.HttpGet get)
        {
            get.ResponsePort.Post(new dssphttp.HttpResponseType(SyncGenericAnalogState()));
            yield break;
        }
        */
        /// <summary>
        /// HttpGet Contact Sensor Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_genericContactPort")]
        public virtual IEnumerator<ITask> HttpGetContactSensorHandler(dssphttp.HttpGet get)
        {
            get.ResponsePort.Post(new dssphttp.HttpResponseType(SyncGenericContactState()));
            yield break;
        }

        /// <summary>
        /// Replace Handler
        /// </summary>
        /// <param name="replace"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_genericContactPort")]
        public virtual IEnumerator<ITask> ReplaceHandler(contactsensor.Replace replace)
        {
            replace.ResponsePort.Post(Fault.FromException(new InvalidOperationException("The HiTechnic Touch Sensor Mux is updated from hardware.")));
            yield break;
        }

        /// <summary>
        /// TouchMuxUpdate Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> TouchMuxUpdateHandler(TouchMuxUpdate update)
        {
            update.ResponsePort.Post(Fault.FromException(new InvalidOperationException("The HiTechnic Touch Sensor Mux is updated from hardware.")));
            yield break;
        }

        /// <summary>
        /// ReliableSubscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ReliableSubscribeHandler(contactsensor.ReliableSubscribe subscribe)
        {
            yield return (Choice)base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
        }

        /// <summary>
        /// Subscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SubscribeHandler(contactsensor.Subscribe subscribe)
        {
            yield return (Choice)base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
            if (_state.TimeStamp != DateTime.MinValue)
            {
                SendNotificationToTarget<contactsensor.Replace>(subscribe.Body.Subscriber, _genericSubMgrPort, SyncGenericContactState());
            }
        }

        /// <summary>
        /// ReliableSubscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericContactPort")]
        public virtual IEnumerator<ITask> GenericReliableSubscribeHandler(contactsensor.ReliableSubscribe subscribe)
        {
            yield return (Choice)base.SubscribeHelper(_genericSubMgrPort, subscribe.Body, subscribe.ResponsePort);
        }

        /// <summary>
        /// Subscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericContactPort")]
        public virtual IEnumerator<ITask> GenericSubscribeHandler(contactsensor.Subscribe subscribe)
        {
            yield return (Choice)base.SubscribeHelper(_genericSubMgrPort, subscribe.Body, subscribe.ResponsePort);
            if (_state.TimeStamp != DateTime.MinValue)
            {
                // Send the initial notifications
                SendNotificationToTarget<TouchMuxUpdate>(subscribe.Body.Subscriber, _subMgrPort, _state);
                // Updates???
                SendNotificationToTarget<contactsensor.Replace>(subscribe.Body.Subscriber, _subMgrPort, SyncGenericContactState());
            }
        }

        /// <summary>
        /// Drop Handler
        /// </summary>
        /// <param name="drop"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Teardown)]
        [ServiceHandler(ServiceHandlerBehavior.Teardown, PortFieldName = "_genericAnalogPort")]
        [ServiceHandler(ServiceHandlerBehavior.Teardown, PortFieldName = "_genericContactPort")]
        public virtual IEnumerator<ITask> DropHandler(DsspDefaultDrop drop)
        {
            // detach from the brick
            _legoBrickPort.Detach(ServiceInfo.Service);

            // drop the service
            base.DefaultDropHandler(drop);
            yield break;
        }


        /// <summary>
        /// ConnectToBrick Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ConnectToBrickHandler(ConnectToBrick update)
        {
            // Validate the sensor port.
            if ((update.Body.SensorPort & NxtSensorPort.AnySensorPort)
                != update.Body.SensorPort)
            {
                update.ResponsePort.Post(
                    Fault.FromException(
                        new ArgumentException(
                            string.Format("Invalid Sensor Port: {0}",
                                ((LegoNxtPort)update.Body.SensorPort)))));
                yield break;
            }

            _state.SensorPort = update.Body.SensorPort;
            _state.Connected = false;
            _state.PollingFrequencyMs = update.Body.PollingFrequencyMs;

            if (!string.IsNullOrEmpty(update.Body.Name))
                _state.Name = update.Body.Name;

            Fault fault = null;

            pxbrick.AttachRequest attachRequest = new pxbrick.AttachRequest(
                new pxbrick.Registration( 
                    new LegoNxtConnection((LegoNxtPort)_state.SensorPort), 
                    LegoDeviceType.AnalogSensor, 
                    Contract.DeviceModel, 
                    Contract.Identifier, 
                    ServiceInfo.Service,
                    _state.Name));

            attachRequest.InitializationCommands = new nxtcmd.NxtCommandSequence(
                new nxtcmd.LegoSetInputMode(_state.SensorPort, LegoSensorType.Custom, LegoSensorMode.RawMode));

            attachRequest.PollingCommands = new nxtcmd.NxtCommandSequence(_state.PollingFrequencyMs,
                new nxtcmd.LegoGetInputValues(_state.SensorPort));

            pxbrick.AttachResponse response = null;

            yield return Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
                delegate(pxbrick.AttachResponse rsp) { response = rsp; },
                delegate(Fault f) { fault = f; });

            _state.Connected = (response != null && (response.Connection.Port != LegoNxtPort.NotConnected));
            if (response == null
                || (!_state.Connected  && update.Body.SensorPort != NxtSensorPort.NotConnected))
            {
                if (fault == null)
                    fault = Fault.FromException(new Exception("Failure Configuring Touch Mux on port: " + update.Body.SensorPort.ToString()));

                update.ResponsePort.Post(fault);
                yield break;
            }

            if (_state.Connected)
            {
                _state.SensorPort = (NxtSensorPort)response.Connection.Port;
                // Set the hardware identifier from the connected sensor port.
                switch (_state.SensorPort)
                {
                    case NxtSensorPort.Sensor1:
                        _sensorPort = 1;
                        break;
                    case NxtSensorPort.Sensor2:
                        _sensorPort = 2;
                        break;
                    case NxtSensorPort.Sensor3:
                        _sensorPort = 3;
                        break;
                    case NxtSensorPort.Sensor4:
                        _sensorPort = 4;
                        break;
                    default:
                        _sensorPort = 0;
                        break;
                }

                // Initialize the Generic ContactSensorState
                _genericContactState.Sensors[0].Name = "Touch Mux " + response.Connection.ToString() + ":1";
                _genericContactState.Sensors[0].HardwareIdentifier = _sensorPort * 10 + 1;
                _genericContactState.Sensors[1].Name = "Touch Mux " + response.Connection.ToString() + ":1";
                _genericContactState.Sensors[1].HardwareIdentifier = _sensorPort * 10 + 2;
                _genericContactState.Sensors[2].Name = "Touch Mux " + response.Connection.ToString() + ":1";
                _genericContactState.Sensors[2].HardwareIdentifier = _sensorPort * 10 + 3;
                _genericContactState.Sensors[3].Name = "Touch Mux " + response.Connection.ToString() + ":1";
                _genericContactState.Sensors[3].HardwareIdentifier = _sensorPort * 10 + 4;

                // Set the sensor name
                if (string.IsNullOrEmpty(_state.Name)
                    || _state.Name.StartsWith("Touch Mux on "))
                    _state.Name = "Touch Mux on " + response.Connection.ToString();

                // Send a connection notification
                update.Body.Name = _state.Name;
                update.Body.PollingFrequencyMs = _state.PollingFrequencyMs;
                update.Body.SensorPort = _state.SensorPort;
                SendNotification<ConnectToBrick>(_subMgrPort, update);
            }

            // Send the message response
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

    }
}
